/**
 * @file     STM32FreeRTOS.c
 * @brief    Based on FreeRTOS for Due and Teensy 3.0
 * @modified by Frederic Pillon <frederic.pillon@st.com> for STMicroelectronics.
 */
#ifdef USE_FREERTOS

#include <STM32FreeRTOS.h>
#include <Arduino.h>
#include <core_fault.h>
#include "rte_data.h"
#ifdef STM32F107xC
#include "stm32f1xx_ll_usart.h"
#elif defined(STM32H750xx)

#include <stm32h7xx_hal.h>


#endif
#ifdef STM32F1xx

#include "stm32f1xx_ll_usart.h"

#endif
#ifdef STM32F407xx

#include "stm32f4xx_ll_usart.h"


#endif
extern unsigned int xFreeBytesRemaining;

char *getTaskName();

void print_thread() {
    UBaseType_t task_num;
    int TotalRunTime;
    TaskStatus_t StatusArray[8];

    task_num = uxTaskGetNumberOfTasks();
    if (StatusArray != NULL) {
        uxTaskGetSystemState(&StatusArray, task_num, (uint32_t *) &TotalRunTime);
    }
    for (int i = 0; i < task_num; i++) {
        TaskStatus_t *t = &StatusArray[i];
        core_debug(
                "%d,%s:xHandle=%p,eCurrentState=%d,uxCurrentPriority=%d,uxBasePriority=%d , ulRunTimeCounter=%d ,pxStackBase =0x%x , usStackHighWaterMark=%d\n",
                t->xTaskNumber, t->pcTaskName, t->xHandle, t->eCurrentState, t->uxCurrentPriority, t->uxBasePriority,
                t->ulRunTimeCounter, t->pxStackBase, t->usStackHighWaterMark);
    }
}
//------------------------------------------------------------------------------
/** delay between led error flashes
 * \param[in] millis milliseconds to delay
 */
static void delayMS(uint32_t millis) {
    uint32_t iterations = millis * (F_CPU / 7000);
    for (volatile int i = 0; i < iterations; ++i) {
        __asm__("nop\n\t");
    }
}

//------------------------------------------------------------------------------
/** Blink error pattern
 *
 * \param[in] n  number of short pulses
 */
//  void errorBlink(int n) {
// #ifdef LED_BUILTIN
//     noInterrupts();
//     pinMode(LED_BUILTIN, OUTPUT);
//     for (int j = 0; j < 10; j++) {
//         for (int i = 0; i < n; i++) {
//             digitalWrite(LED_BUILTIN, 1);
//             delayMS(300);
//             digitalWrite(LED_BUILTIN, 0);
//             delayMS(300);
//         }
//         delayMS(2000);
//     }
// #else
//     while (1);
// #endif // LED_BUILTIN
// }

//------------------------------------------------------------------------------
/** assertBlink
 * Blink one short pulse every two seconds if configASSERT fails.
 */
void assertBlink() {
    errorBlink(1);
}
//------------------------------------------------------------------------------
#if (configUSE_MALLOC_FAILED_HOOK == 1)

/** vApplicationMallocFailedHook()
   Blink two short pulses if malloc fails.

    will only be called if
    configUSE_MALLOC_FAILED_HOOK is set to 1 in FreeRTOSConfig.h.  It is a hook
    function that will get called if a call to pvPortMalloc() fails.
    pvPortMalloc() is called internally by the kernel whenever a task, queue,
    timer or semaphore is created.  It is also called by various parts of the
    demo application.  If heap_1.c or heap_2.c are used, then the size of the
    heap available to pvPortMalloc() is defined by configTOTAL_HEAP_SIZE in
    FreeRTOSConfig.h, and the xPortGetFreeHeapSize() API function can be used
    to query the size of free heap space that remains (although it does not
    provide information on how the remaining heap might be fragmented). */
void vApplicationMallocFailedHook() {
    core_debug("malloc Failed. thread=%s ,memremain=%d", getTaskName(), xFreeBytesRemaining);
//    HardFault_Handler();
}

#endif /* configUSE_MALLOC_FAILED_HOOK == 1 */

//------------------------------------------------------------------------------
#if (configUSE_IDLE_HOOK == 1)

/** vApplicationIdleHook() will only be called if configUSE_IDLE_HOOK is set
    to 1 in FreeRTOSConfig.h.  It will be called on each iteration of the idle
    task.  It is essential that code added to this hook function never attempts
    to block in any way (for example, call xQueueReceive() with a block time
    specified, or call vTaskDelay()).  If the application makes use of the
    vTaskDelete() API function (as this demo application does) then it is also
    important that vApplicationIdleHook() is permitted to return to its calling
    function, because it is the responsibility of the idle task to clean up
    memory allocated by the kernel to any task that has since been deleted. */
void loop();

void __attribute__((weak)) vApplicationIdleHook(void) {
    loop();
}

#endif /* configUSE_IDLE_HOOK == 1 */

/*-----------------------------------------------------------*/
#if (configCHECK_FOR_STACK_OVERFLOW >= 1)

/**  Blink three short pulses if stack overflow is detected.
    Run time stack overflow checking is performed if
    configCHECK_FOR_STACK_OVERFLOW is defined to 1 or 2.  This hook
    function is called if a stack overflow is detected.
  \param[in] pxTask Task handle
  \param[in] pcTaskName Task name
  */
void vApplicationStackOverflowHook(TaskHandle_t pxTask, char *pcTaskName) {
    // (void) pcTaskName;
    (void) pxTask;
    core_debug("stack overflow:%s\n", pcTaskName);
}

#endif /* configCHECK_FOR_STACK_OVERFLOW >= 1 */

//------------------------------------------------------------------------------
// catch exceptions
/** Hard fault - blink four short flash every two seconds */
void hard_fault_isr() {
    // core_debug("Hard fault isr\n");
    errorBlink(4);
}


void prvGetRegistersFromStack(uint32_t *pulFaultStackAddress) {
    /* These are volatile to try and prevent the compiler/linker optimising them
    away as the variables never actually get used.  If the debugger won't show the
    values of the variables, make them global my moving their declaration outside
    of this function. */
    volatile uint32_t r0;
    volatile uint32_t r1;
    volatile uint32_t r2;
    volatile uint32_t r3;
    volatile uint32_t r12;
    volatile uint32_t lr;  /* Link register. */
    volatile uint32_t pc;  /* Program counter. */
    volatile uint32_t psr; /* Program status register. */

    r0 = pulFaultStackAddress[0];
    r1 = pulFaultStackAddress[1];
    r2 = pulFaultStackAddress[2];
    r3 = pulFaultStackAddress[3];

    r12 = pulFaultStackAddress[4];
    lr = pulFaultStackAddress[5];
    pc = pulFaultStackAddress[6];
    psr = pulFaultStackAddress[7];
    core_debug("\n[Hard fault handler]\n");
    core_debug("R0 = 0x%x\n", r0);
    core_debug("R1 = 0x%x\n", r1);
    core_debug("R2 = 0x%x\n", r2);
    core_debug("R3 = 0x%x\n", r3);
    core_debug("R12 = 0x%x\n", r12);
    core_debug("LR[R14] = 0x%x\n", lr);
    core_debug("PC[R15] = 0x%x\n", pc);
    core_debug("PSR = 0x%x\n", psr);
    core_debug("BFAR = 0x%x\n", (*((volatile unsigned long *) (0xE000ED38))));
    core_debug("CFSR = 0x%x\n", (*((volatile unsigned long *) (0xE000ED28))));
    core_debug("HFSR = 0x%x\n", (*((volatile unsigned long *) (0xE000ED2C))));
    core_debug("DFSR = 0x%x\n", (*((volatile unsigned long *) (0xE000ED30))));
    core_debug("AFSR = 0x%x\n", (*((volatile unsigned long *) (0xE000ED3C))));
    core_debug("SCB_SHCSR = 0x%x\n", SCB->SHCSR);
    core_debug("xFreeBytesRemaining = %d\n", xFreeBytesRemaining);
    core_debug("task = %s\n", getTaskName());
    // print_thread();
    errorBlink(5);
}

/** Hard fault - blink four short flash every two seconds */
void HardFault_Handler() {
    // print_thread();
    __asm volatile(
            " tst lr, #4                                                \n"
            " ite eq                                                    \n"
            " mrseq r0, msp                                             \n"
            " mrsne r0, psp                                             \n"
            " ldr r1, [r0, #24]                                         \n"
            " b prvGetRegistersFromStack                                \n");
}

void NMI_Handler() {
    errorBlink(6);
}

void MemManage_Handler() {
    __asm volatile(
            " tst lr, #4                                                \n"
            " ite eq                                                    \n"
            " mrseq r0, msp                                             \n"
            " mrsne r0, psp                                             \n"
            " ldr r1, [r0, #24]                                         \n"
            " b prvGetRegistersFromStack                                \n");
}

/** Bus fault - blink five short flashes every two seconds */
void bus_fault_isr() {
    errorBlink(8);
}

/** Bus fault - blink five short flashes every two seconds */
void BusFault_Handler() {
    errorBlink(8);
}

/** Usage fault - blink six short flashes every two seconds */
void usage_fault_isr() {
    errorBlink(9);
}

/** Usage fault - blink six short flashes every two seconds */
void UsageFault_Handler() {
    errorBlink(9);
}

/*-----------------------------------------------------------*/
#if (configUSE_TICK_HOOK == 1)
/** This function will be called by each tick interrupt if
    configUSE_TICK_HOOK is set to 1 in FreeRTOSConfig.h.  User code can be
    added here, but the tick hook is called from an interrupt context, so
    code must not attempt to block, and only the interrupt safe FreeRTOS API
    functions can be used (those that end in FromISR()). */
void __attribute__((weak)) vApplicationTickHook()
{
}
#endif /* configUSE_TICK_HOOK == 1 */

/*-----------------------------------------------------------*/
/** Dummy time stats gathering functions need to be defined to keep the
linker happy.  Could edit FreeRTOSConfig.h to remove these.*/
void vMainConfigureTimerForRunTimeStats(void) {
    // not use
}

/** Dummy function
 *  \return zero
 */
unsigned long ulMainGetRunTimeCounterValue() { return 0UL; }

#endif
